﻿using System;
using System.Threading;
using TrioMotion.TrioPC_NET;

namespace TrioP600
{
    public class TrioP600
    {
        TrioPC trio = new TrioPC(); // 实例化一个TrioPC对象
        /// 轴参数初始化
        /// </summary>
        private void AxisInit()
        {
            //初始化轴0,1,4
            int i = 0;
            trio.SetAxisVariable("UNITS", i, 13107.2);
            trio.SetAxisVariable("SPEED", i, 0); // 设置轴速度
            trio.SetAxisVariable("ACCEL", i, 2500); // 设置加速度
            trio.SetAxisVariable("DECEL", i, 2500); // 设置减速度
            trio.SetAxisVariable("SERVO", i, 1); // SERVO=1:进行闭环运动，SERVO=0：开环运动

            i = 1;
            trio.SetAxisVariable("UNITS", i, 1);

            i = 4;
            trio.SetAxisVariable("UNITS", i, 13107.2);
            trio.SetAxisVariable("SPEED", i, 0); // 设置轴速度
            trio.SetAxisVariable("ACCEL", i, 2500); // 设置加速度
            trio.SetAxisVariable("DECEL", i, 2500); // 设置减速度
            trio.SetAxisVariable("SERVO", i, 1); // SERVO=1:进行闭环运动，SERVO=0：开环运动
        }
        /// <summary>
        /// 检查EC状态
        /// </summary>
        /// <returns></returns>
        private double CheckECState()
        {
            double state = -1;
            // 判断EC状态，若异常则进行相应的操作
            trio.SetVr(0, -1); // 初始化VR(0)=-1，用于存放EC的状态
            trio.Execute("ETHERCAT($22,0,0)"); // 将EtherCAT的状态返回到VR(0)中。EtherCAT指令详见Trio BASIC
            trio.GetVr(0, out state);
            int i = 0;
            while (state != 3 && (i < 3))// 控制器未连接驱动器，则重新初始化EC
            {
                trio.Execute("ETHERCAT(0,0)"); // 重新初始化EC
                Thread.Sleep(3000);
                trio.Execute("ETHERCAT($22,0,0)");
                trio.GetVr(0, out state);
                i++;
            }
            return state;
        }
        //编码器读取
        public double GetEncoder()//编码器采集原数据，和单位挂钩
        {
            double position;
            trio.GetAxisVariable("MPOS", 1, out position); // 获取轴4的实际位置
            return position;
        }
        public void InitMpos()//编码器清零、磁粉初始力
        {
            trio.Defpos(0, 1);//编码器清零
            SetTension(0, 256);
            SetTension(4, 256);
        }
        //模拟量输出
        public void SetTension(int axis, double tension)//磁粉输出轴和数值
        {
            double b;
            trio.Aout(axis, tension, out b);
        }

        /// <summary>
        /// 建立远程同步连接，并初始化轴参数
        /// </summary>
        public bool Connect()//控制器连接
        {
            string motorip = "192.168.0.250";
            double state;
            trio.SetHost(motorip);
            if (!trio.IsOpen(PortId.EthernetREMOTE)) // 判断程序是否与控制器之间建立了远程同步连接，没有则建立连接
            {
                trio.Open(PortType.Ethernet, PortId.EthernetREMOTE);// 打开程序与控制器的远程连接（同步模式）
            }
            if (trio.IsOpen(PortId.EthernetREMOTE)) ;
            {
                string IPStart = motorip.Substring(0, 3);
                if (IPStart == "127") //如果使用的是仿真器不需要进行EC状态检查
                {
                    state = 3;
                }
                else
                {
                    state = CheckECState(); //否则，需要进行EC状态检查。（前提是使用的EC总线）
                }
                if (state == 3) //EC正常
                {
                    AxisInit(); // 轴参数设置
                    return true;
                }
                else
                {
                    return false;
                }
            }
        }
        public bool InitMotor(int axis)
        {
            try
            {
                double value, state = -1;
                trio.SetVariable("WDOG", 1);
                trio.GetVariable("WDOG", out value);
                if (value != 1)// 若设置失败，可能是EC总线异常，此时重新初始化EC
                {
                    state = CheckECState(); //如果是EC通讯方式，进行EC状态检测
                    if (state == 3) //EC正常
                    {
                        trio.SetVariable("WDOG", 1); // 伺服使能
                    }
                }
                trio.Cancel(2, axis);  //取消当前轴所有运动
                if (axis == 0)
                {
                    trio.Reverse(axis);
                }
                else if (axis == 4)
                {
                    trio.Forward(axis);
                }
                return true;
            }
            catch (Exception)
            {
                return false;
            }
        }
        public void StopMove()//停止运动
        {
            CtrlSpeed(0, 0);
            CtrlSpeed(4, 0);//速度置为0
            trio.RapidStop(); // 取消所有的轴的当前运动
        }
        public void Closed()//关闭连接，formclose
        {
            trio.RapidStop(); // 取消所有的轴的当前运动
            trio.SetVariable("WDOG", 0);//使能关闭
            trio.Close(PortId.EthernetREMOTE);// 关闭连接
        }
        public void CtrlSpeed(int axis, double speed)//速度控制
        {
            if (speed < 0)
            {
                speed = 0;
            }
            trio.SetAxisVariable("SPEED", axis, speed);
        }
    }
}
